/* Control.cc -- Faz a interface entre servidor e robôs
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 *
 * This file is part of GEARMedula.
 * 
 * GEARMedula is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, version 3 of the
 * License.
 * 
 * GEARMedula is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#include <iostream>
#include <string>

#include "Control.hh"

using namespace std;

#define NUMBER_OF_ROBOTS 5

/** Endereço dos robôs para o hardware  */
#define GM_CONTROL_END_ROBOT0 "BAL"
#define GM_CONTROL_END_ROBOT1 "BA1"
#define GM_CONTROL_END_ROBOT2 "BA2"
#define GM_CONTROL_END_ROBOT3 "BA3"
#define GM_CONTROL_END_ROBOT4 "BA4"

Control::Control()
{
}

Control::~Control()
{
    // for( int i = 0 ; i < NUMBER_OF_ROBOTS ; i++ )
    // {
    // 	delete box_[i];
    // }
}

void
Control::Init( const int f_real,
	       const string serial_path)
{
    int i = 0;

    box_.resize(5);
    f_real_ = f_real;

    /* Se o Servidor for real */
    if( f_real_ )
    {
	/* Criando robô 0 */
	box_[0] = new ControlRobot();
	box_[0]->Init( GM_CONTROL_END_ROBOT0, serial_path );	

	/* Criando robô 1 */
	box_[1] = new ControlRobot();
	box_[1]->Init( GM_CONTROL_END_ROBOT1, serial_path );
	
	/* Criando robô 2 */
	box_[2] = new ControlRobot();
	box_[2]->Init( GM_CONTROL_END_ROBOT2, serial_path );
	
	/* Criando robô 3 */
	box_[3] = new ControlRobot();
	box_[3]->Init( GM_CONTROL_END_ROBOT3, serial_path );
	
	/* Criando robô 4 */
	box_[4] = new ControlRobot();
	box_[4]->Init( GM_CONTROL_END_ROBOT4, serial_path );
    }
    
}

void
Control::setSim( GMSim * sim )
{
    sim_ = sim;
}

ControlRobot::ControlRobot()
{
    
}

ControlRobot::~ControlRobot()
{
    
}

void
ControlRobot::Init( const string end, const string serial_path )
{
    serial_.Init( end, serial_path);
    serial_path_ = serial_path;
}

void
ControlRobot::SetVel( const float vx, const float vy, const float va)
{
    serial_.robotSetVel( vx, vy, va );
}

void
ControlRobot::Kick( const int nivel)
{
    serial_.robotKick( nivel );
}

void
ControlRobot::DribbleEnable( const int enable)
{
    serial_.robotDribbleEnable( enable );
}

void
Control::robotSetVel( const int id_robot,
		      const float vx,
		      const float vy,
		      const float va)
{
    if( f_real_ )
    {
	box_.at(id_robot)->SetVel( vx, vy, va );
    }
    else
    {
	sim_->robot_set_vel( id_robot, vx, vy, va );
    }
}

void
Control::robotKick( const int id_robot,
		    const int nivel)
{
    if( f_real_ )
    {
	box_.at(id_robot)->Kick( nivel );
    }
    else
    {
	sim_->robot_kick(id_robot, nivel);
    }
}

void
Control::robotDribbleEnable( const int id_robot,
			     const int enable )
{
    if( f_real_ )
    {
	box_.at(id_robot)->DribbleEnable( enable );
    }
    else
    {
	sim_->robot_dribble_enable(id_robot, enable);
    }
}

